package kit.edu.lego.gruppe3.levels.sumpf;




import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.levels.LevelSolver;
import kit.edu.lego.gruppe3.levels.LevelSelector.Level;
import lejos.nxt.NXTRegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

public class SumpfSolver implements LevelSolver{
	
	private static NXTRegulatedMotor motor = ComponentsRepository.getInstance().getMiddleMotor();
	private static DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();
	
	@Override
	public void start() {
		SumpfSolver.initSumpfPosition();
		
		LevelSelector.getInstance().setCurrentLevel(Level.sumpf);

		Behavior darkGround = new DarkGround();
		Behavior lightGround = new LightGround();
		Behavior hitLeftWall = new LeftWallHit();
		Behavior distance = new DistanceSensor();
			
		Behavior[] behaviors = {lightGround,darkGround, distance, hitLeftWall};
			
		Arbitrator arby = new Arbitrator(behaviors);
		arby.start();
		
	}
	
	public static void initSumpfPosition(){
		motor.stop(false);
		robot.setTravelSpeed(12);
		robot.rotate(95);
		while (true){
			boolean leftTouchPressed = ComponentsRepository.getInstance().getLeftTouchSensor().isPressed();
			boolean righTouchPressed = ComponentsRepository.getInstance().getRightTouchSensor().isPressed();
			
			if (leftTouchPressed && righTouchPressed){
				robot.rotate(90);
				break;
			} else {
				if (leftTouchPressed){
					robot.rotate(-85);
					break;
				}
				
				if (righTouchPressed){
					robot.rotate(-100);
					break;
				}
			}
		
			robot.forward();
		}
		robot.setTravelSpeed(7);
		
		motor.rotate( (-1) * ComponentsRepository.INIT_SENSORS_ANGLE);
		motor.stop();
		
	}
	
	private static boolean levelCleared = false;
	
	public static boolean isLevelCleared() {
		return levelCleared;
	}
	
	public static void setLevelCleared(boolean cleared){
		levelCleared = cleared;
	}
	
	public String toString() {
		return "Sumpfsolver";
	}

}
